#!/usr/bin/python3
# import rospy
# from sensor_msgs.msg import LaserScan
# import cv2
# import numpy as np
# import math
# from sensor_msgs.msg import LaserScan




# def callback(data):
 
#     point_list = []
#     frame = np.zeros((600, 600,3), np.uint8)
#     angle = data.angle_min
#     for r in data.ranges:

#         if math.isinf(r) == True:
#             r = 0
      
#         #x,y:相对于无人机坐标
#         x = math.trunc((r * 50.0)*math.cos(angle + (-90.0*3.1416/180.0)))
#         y = math.trunc((r * 50.0)*math.sin(angle + (-90.0*3.1416/180.0)))
#         point_dict = {'r' : r, 'x' : x, 'y' : y, 'angle' : angle * 180 / math.pi}
#         if r != 0:
#             point_list.append(point_dict)
#             cv2.circle(frame,(x+300,y+300),1,(0,255,0),-1)
# 		# 角度得增加
#         angle= angle + data.angle_increment 
#         # 画个中心圆 
#         cv2.circle(frame, (300, 300), 3, (255, 255, 255))
#     # print(point_list)

#     sorted_point_list = sorted(point_list, key=lambda x: x['r'])
#     print(sorted_point_list[0])
#     cv2.imshow('frame',frame)
#     cv2.waitKey(1)

# def laser_listener():
#     rospy.init_node('laser_listener', anonymous=True)
#     rospy.Subscriber("/scan", LaserScan,callback,queue_size = 1)
#     rospy.spin()

# if __name__ == '__main__':
#     laser_listener()

# #!/usr/bin/python3
# import rospy
# import cv2
# import math
# import numpy as np
# from sensor_msgs.msg import LaserScan

# def find_valleys(ranges, angle_min, angle_increment, window_size=5):
#     """
#     基于滑动窗口的极坐标谷值检测算法[6,7](@ref)
#     参数：
#         ranges : 激光雷达距离数组
#         angle_min : 起始弧度
#         angle_increment : 角度步长
#         window_size : 检测窗口大小(奇数)
#     返回：
#         谷值数组[[r1, angle1], [r2, angle2], ...]
#     """
#     valleys = []
#     half_window = window_size // 2
    
#     # 预处理：替换inf为max_range[7](@ref)
#     max_range = max([r for r in ranges if not math.isinf(r)])
#     processed_ranges = [r if not math.isinf(r) else max_range for r in ranges]
    
#     # 滑动窗口检测[6](@ref)
#     for i in range(len(processed_ranges)):
#         start = max(0, i - half_window)
#         end = min(len(processed_ranges), i + half_window + 1)
#         window = processed_ranges[start:end]
        
#         if processed_ranges[i] == min(window):
#             angle = angle_min + i * angle_increment
#             valleys.append({
#                 'r': processed_ranges[i],
#                 'angle': math.degrees(angle)  # 转换为角度制
#             })
#     return valleys

# def callback(data):
#     # 初始化画布
#     frame = np.zeros((600, 600, 3), np.uint8)
    
#     # 获取雷达参数[7](@ref)
#     params = {
#         'angle_min': data.angle_min,
#         'angle_max': data.angle_max,
#         'range_max': data.range_max if data.range_max > 0 else 20.0
#     }
    
#     # 谷值检测[6](@ref)
#     valleys = find_valleys(data.ranges, 
#                          params['angle_min'],
#                          data.angle_increment,
#                          window_size=5)
    
#     # 绘制原始数据及谷值点
#     prev_point = None
#     for i, r in enumerate(data.ranges):
#         if math.isinf(r) or r < data.range_min:
#             continue
            
#         current_angle = params['angle_min'] + i * data.angle_increment
#         x = int((current_angle - params['angle_min']) / 
#                (params['angle_max'] - params['angle_min']) * 599)
#         y = int(599 - (r / params['range_max']) * 599)
        
#         # 绘制原始数据（青色轨迹）
#         cv2.circle(frame, (x, y), 1, (255, 255, 0), -1)
#         if prev_point:
#             cv2.line(frame, prev_point, (x, y), (255, 255, 0), 1)
#         prev_point = (x, y)
    
#     # 高亮显示谷值点（红色标记）[6](@ref)
#     for valley in valleys:
#         angle_rad = math.radians(valley['angle'])
#         x = int((angle_rad - params['angle_min']) / 
#                (params['angle_max'] - params['angle_min']) * 599)
#         y = int(599 - (valley['r'] / params['range_max']) * 599)
#         cv2.circle(frame, (x, y), 5, (0, 0, 255), -1)
    
#     # 显示统计信息
#     cv2.putText(frame, f"Valleys: {len(valleys)}", (10, 30),
#                cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
    
#     cv2.imshow('Lidar Scan with Valleys', frame)
#     cv2.waitKey(1)

# def laser_listener():
#     rospy.init_node('laser_listener', anonymous=True)
#     rospy.Subscriber("/scan", LaserScan, callback, queue_size=1)
#     rospy.spin()

# if __name__ == '__main__':
#     laser_listener()




# #!/usr/bin/python3
# import rospy
# import cv2
# import math
# import numpy as np
# from sensor_msgs.msg import LaserScan


# def callback(data):
#     # 初始化画布
#     frame = np.zeros((600, 600, 3), np.uint8)
    
#     # 获取雷达参数[7](@ref)
#     params = {
#         'angle_min': data.angle_min,
#         'angle_max': data.angle_max,
#         'range_max': data.range_max if data.range_max > 0 else 20.0
#     }
    
#     # 绘制原始数据及谷值点
#     prev_point = None
#     for i, r in enumerate(data.ranges):
#         if math.isinf(r) or r < data.range_min:
#             continue
            
#         current_angle = params['angle_min'] + i * data.angle_increment
#         x = int((current_angle - params['angle_min']) / 
#                (params['angle_max'] - params['angle_min']) * 599)
#         y = int(599 - (r / params['range_max']) * 599)
        
#         # 绘制原始数据（青色轨迹）
#         cv2.circle(frame, (x, y), 1, (255, 255, 0), -1)
#         if prev_point:
#             cv2.line(frame, prev_point, (x, y), (255, 255, 0), 1)
#         prev_point = (x, y)
    
    
#     cv2.imshow('Lidar Scan with Valleys', frame)
#     cv2.waitKey(1)

# def laser_listener():
#     rospy.init_node('laser_listener', anonymous=True)
#     rospy.Subscriber("/scan", LaserScan, callback, queue_size=1)
#     rospy.spin()

# if __name__ == '__main__':
#     laser_listener()



#!/usr/bin/python3
import rospy
import cv2
from sensor_msgs.msg import LaserScan
import math
import numpy as np
from drone_pkg.msg import Raogan

MIN_distance = 1

def forward_scan(ranges, angle_min, angle_increment, range_min, range_max, change_threshold=0.5):
    valid_ranges = [r for r in ranges if not math.isinf(r) and r >= range_min]
    avg_range = np.mean(valid_ranges) if valid_ranges else 0

    valleys = []
    prev_range = None

    for i, r in enumerate(ranges):
        if math.isinf(r) or r < range_min:
            continue

        current_angle = angle_min + i * angle_increment
        x = int((current_angle - angle_min) / (2 * math.pi) * 599)  # 映射到图像坐标
        y = int(599 - (r / range_max) * 599)  # 映射到图像坐标

        # 检测变化量，且当前值小于平均值
        if prev_range is not None:
            if abs(r - prev_range) > change_threshold and r < avg_range:
                valleys.append({'r': r, 'angle': current_angle, 'x': x, 'y': y})

        prev_range = r

    return valleys

def backward_scan(ranges, angle_min, angle_increment, range_min, range_max, change_threshold=0.5):
    valid_ranges = [r for r in ranges if not math.isinf(r) and r >= range_min]
    avg_range = np.mean(valid_ranges) if valid_ranges else 0

    valleys = []
    prev_range = None

    # 从后向前遍历
    for i in range(len(ranges) - 1, -1, -1):
        r = ranges[i]
        if math.isinf(r) or r < range_min:
            continue

        current_angle = angle_min + i * angle_increment
        x = int((current_angle - angle_min) / (2 * math.pi) * 599)  # 映射到图像坐标
        y = int(599 - (r / range_max) * 599)  # 映射到图像坐标

        # 检测变化量，且当前值小于平均值
        if prev_range is not None:
            if abs(r - prev_range) > change_threshold and r < avg_range:
                valleys.append({'r': r, 'angle': current_angle, 'x': x, 'y': y})

        prev_range = r

    return valleys

def merge_valleys(forward_valleys, backward_valleys, angle_diff_threshold=4):
    merged_valleys = []

    for forward in forward_valleys:
        for backward in backward_valleys:
            angle_diff = abs(forward['angle'] - backward['angle']) * (180 / math.pi)  # 转换为度
            if angle_diff < angle_diff_threshold:
                merged_valley = {
                    'r': (forward['r'] + backward['r']) / 2,
                    'angle': (forward['angle'] + backward['angle']) / 2,
                    'x': (forward['x'] + backward['x']) // 2,
                    'y': (forward['y'] + backward['y']) // 2,
                    'type': 'merged'  # 合并的谷值
                }
                merged_valleys.append(merged_valley)

    return merged_valleys


def find_valleys(ranges, angle_min, angle_increment, range_min, range_max, change_threshold=0.5):
    forward_valleys = forward_scan(ranges, angle_min, angle_increment, range_min, range_max, change_threshold)
    backward_valleys = backward_scan(ranges, angle_min, angle_increment, range_min, range_max, change_threshold)
    merged_valleys = merge_valleys(forward_valleys, backward_valleys)

    return forward_valleys, backward_valleys, merged_valleys


def callback(data):
    global MIN_distance, gan_r, gan_angle, rao_gan_pub, raogan_msg
    # 初始化画布
    frame = np.zeros((600, 600, 3), np.uint8)
    
    # 获取雷达参数
    params = {
        'angle_min': data.angle_min,
        'angle_max': data.angle_max,
        'range_max': data.range_max if data.range_max > 0 else 20.0,
        'angle_increment': data.angle_increment,
        'range_min': data.range_min
    }
    
    # 获取谷值
    _, _, valleys = find_valleys(
        data.ranges,
        params['angle_min'],
        params['angle_increment'],
        params['range_min'],
        params['range_max']
    )
    
    # 如果有谷值，选择r最小的谷值
    
        
    # 绘制原始数据（青色轨迹）
    prev_point = None
    for i, r in enumerate(data.ranges):
        if math.isinf(r) or r < data.range_min:
            continue
        
        current_angle = params['angle_min'] + i * data.angle_increment
        x = int((current_angle - params['angle_min']) / 
                (params['angle_max'] - params['angle_min']) * 599)
        y = int(599 - (r / params['range_max']) * 599)
        
        # 绘制原始数据（青色轨迹）
        cv2.circle(frame, (x, y), 1, (255, 255, 0), -1)
        if prev_point:
            cv2.line(frame, prev_point, (x, y), (255, 255, 0), 1)
        prev_point = (x, y)
    if len (valleys) != 0:
        min_valley = min(valleys, key=lambda x: x['r'])  # 找到r最小的谷值
    # 高亮显示r最小的谷值（红色标记）
        
        if (min_valley['r'] <= MIN_distance and 
    (
        (-math.pi / 3 <= min_valley['angle'] <= math.pi / 3) or  # 区间 (-60°, 60°)
        (-math.pi <= min_valley['angle'] <= -2 * math.pi / 3) or  # 区间 (-180°, -120°)
        (2 * math.pi / 3 <= min_valley['angle'] <= math.pi)  # 区间 (120°, 180°)
    )
        ):
            cv2.circle(frame, (min_valley['x'], min_valley['y']), 5, (0, 0, 255), -1)  # 红色表示最小r的谷值
            
            gan_r = min_valley['r']
            gan_angle = min_valley['angle'] * 180 / math.pi
            
        else:
            gan_r = -1
    else:
        gan_r = -1
    rospy.loginfo("r:%.2f, angle:%.2f", gan_r, gan_angle)
    raogan_msg.r = gan_r
    raogan_msg.angle = gan_angle
    rao_gan_pub.publish(raogan_msg)
        
    
        

                

    # 显示处理后的图像
    cv2.imshow('Lidar Scan with Valleys', frame)
    cv2.waitKey(1)



gan_r = -1
gan_angle = -1
rao_gan_pub = rospy.Publisher('/raogan', Raogan, queue_size=10)
raogan_msg = Raogan()

def laser_listener():
    global gan_r, gan_angle
    rospy.init_node('laser_listener', anonymous=True)
    rospy.Subscriber("/scan", LaserScan, callback, queue_size=1)

    
    rospy.spin()
    

if __name__ == '__main__':
    laser_listener()
